#ifndef RRT_IMP_H
#define RRT_IMP_H

#include "Collision_detector.h"

class Rrt_Imp
{
public:

	Rrt_Imp (Collision_detector* col):
	  m_collision(col)
	{}

	/**
	 * imp(list<Point>	route)
	 * naive path improve
	 * get path and return an improve path if possioble - try to remove points
	 * O(n^2) - run no all points in the route and try to connect them
	 * source - Kiril Solovy
	 */
	list<Point> imp(list<Point>	route)
	{
		bool imp_done=false;
		vector<Point> vec(route.begin(), route.end());
		list <Point> imp_route;	
		if (vec.size() == 0 || vec[0] == vec[vec.size()-1]) 
		{
			return list<Point>(); 
		}
		Point target = vec[vec.size()-1];
		
		for(int i=0 ; i<vec.size();i++)
		{//run over all the point and try to connect them in down order
			
			for(int j=vec.size()-1 ; j>i+1;j--)
			{
				Point point_new = m_collision->steer(vec[i],vec[j]);
				//check x y coordinate
				if(point_new[0] == vec[j][0] && point_new[1] == vec[j][1] )
				{
					if(point_new[2] != vec[j][2] )
					{//if the robot is not angle aligned - try to turn
						
						if(!m_collision->reach_angle(point_new,vec[j]))
						{
							continue;
						}
						vec.insert(vec.begin()+i+1,point_new);
						vec[i+2]=vec[j+1];
						vec.erase(vec.begin()+i+3,vec.begin()+j+1);
					}
					else
					{//if the robot is exacly on the Point
						vec[i+1]=vec[j];
						vec.erase(vec.begin()+i+2,vec.begin()+j+1);
					}
					imp_done=true;
					i=j;
					break;
				}
			}
		}
		if(imp_done)
		{//copy the leftover point to the new route
			std::copy (vec.begin (), vec.end (), std::back_inserter (imp_route));
		}
		else
		{
			imp_route=route;
		}
		return imp_route;
	}

private:
	Collision_detector*		m_collision;
};


#endif
